import numpy as np
import cv2
from numpy.linalg import lstsq

def calLeftLineFromCameramtx(P, ImgPointSet, apertureWidth):
    '''
    作者：SXL；
    日期：2022.9.5；
    功能：由左相机内参求解相机光心到特征点的直线方程；
    输入：左相机内参、特征点质心坐标ImgPointSet和传感器尺寸apertureWidth;
    输出：该点在世界坐标系下的直线方程lac;
    DEMO：

    '''
    f = P[0] * apertureWidth
    # 像素坐标系到相机坐标系
    mtx=np.array([[P[0],0,P[1]],
                  [0,P[2],P[3]],
                  [0,0,1]])
    Ac = np.linalg.inv(mtx) * (-f) * (np.mat(np.append(ImgPointSet, [1], axis=0))).T
    Ac = np.array([Ac.tolist()[0][0], Ac.tolist()[1][0], Ac.tolist()[2][0]])
    Oc = np.array([0, 0, 0])
    lac = (Ac-Oc) / np.linalg.norm(Ac-Oc)
    return lac

def calRightLineFromCameramtx(P, ImgPointSet, apertureWidth):
    '''
    作者：SXL；
    日期：2022.9.5；
    功能：由右相机内参求解相机光心到特征点的直线方程；
    输入：右相机内参、特征点质心坐标ImgPointSet和传感器尺寸apertureWidth;
    输出：该点在世界坐标系下的直线方程lbc;
    DEMO：

    '''
    f = P[4] * apertureWidth
    mtx=np.array([[P[4],0,P[5]],
                  [0,P[6],P[7]],
                  [0,0,1]])
    Ac = np.linalg.inv(mtx) * (-f) * (np.mat(np.append(ImgPointSet, [1], axis=0))).T
    Ac = np.array([Ac.tolist()[0][0], Ac.tolist()[1][0], Ac.tolist()[2][0]])
    Oc = np.array([0, 0, 0])
    lbc = (Ac-Oc) / np.linalg.norm(Ac-Oc)
    return lbc


def calPointCoordinateFromMatrixandRT(P,ul,vl,ur,vr):
    '''
    作者：SXL；
    日期：2022.9.5；
    功能：由左右相机质心坐标(u,v)求解世界坐标系下对应点坐标(x,y,z)；
    输入：左相机映射矩阵(M_LL)和质心坐标(ul,vl)、右相机映射矩阵(M_LR)和质心坐标(ur,vr);
    输出：该点在世界坐标系下的三维坐标(x,y,z);
    DEMO：

    '''

    UV = np.array([[ul, vl, ur, vr]])
    A = np.array([[UV[0,0] *  0 - P[0], UV[0,0] * 0 - 0, UV[0,0] * 1 - P[1]],
                 [UV[0,1] * 0 - 0, UV[0,1]  * 0 - P[2], UV[0,1]  * 1 - P[3]],
                 [UV[0,2]  * P[14]- (P[4]*P[8]+P[5]*P[14]), UV[0,2]  * P[15] -(P[4]*P[9]+P[5]*P[15]), UV[0,2]  * P[16] -  (P[4]*P[10]+P[5]*P[16])],
                 [UV[0,3]  * P[14]- (P[6]*P[11]+P[7]*P[14]), UV[0,3]  *P[15] -(P[6]*P[12]+P[7]*P[15]), UV[0,3]  * P[16] -  (P[6]*P[13]+P[7]*P[16])]])


    b = np.array([[0 - UV[0,0] * 0], [0 - UV[0,1] * 0], [(P[4]*P[17]+P[5]*P[19]) - UV[0,2] * P[19]],
             [(P[6]*P[18]+P[7]*P[19])- UV[0,3] * P[19]]])
    XYZ = np.dot(np.dot(np.linalg.inv(np.dot(A.T, A)), A.T), b)
    return(XYZ)

